//做实验用三轴力配合gravity_compensation2.py和force_three6.py使用
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <chrono>
#include <vector>
#include <atomic>

using namespace std::chrono_literals;

class PeriodicMover {
public:
  PeriodicMover(const rclcpp::Node::SharedPtr& node, const std::string& group_name = "zc4")
  : node_(node), busy_(false), idx_(0)
  {
    move_group_ = std::make_unique<moveit::planning_interface::MoveGroupInterface>(node_, group_name);
    move_group_->setPlanningTime(12.0);
    move_group_->setNumPlanningAttempts(5);
    move_group_->setMaxVelocityScalingFactor(0.4);
    move_group_->setMaxAccelerationScalingFactor(0.4);

    planning_frame_ = move_group_->getPlanningFrame();
    waypoints_ = makeWaypoints(planning_frame_);

    timer_ = node_->create_wall_timer(5s, std::bind(&PeriodicMover::tick, this));
  }

private:
  static std::vector<geometry_msgs::msg::PoseStamped>
  makeWaypoints(const std::string& frame_id)
  {
    std::vector<geometry_msgs::msg::PoseStamped> wps;
    wps.resize(10);

    // === 轨迹点 1 ===
    wps[0].header.frame_id = frame_id;
    // TODO: 填写 position (米)
    wps[0].pose.position.x = -0.101194;
    wps[0].pose.position.y = 0.0;
    wps[0].pose.position.z = 0.527219;
    // TODO: 填写 orientation 四元数 (x,y,z,w) —— 请确保归一化
    wps[0].pose.orientation.x = 0.0;
    wps[0].pose.orientation.y = -0.45942;
    wps[0].pose.orientation.z = 0.0;
    wps[0].pose.orientation.w = 0.88822;

    // === 轨迹点 2 ===
    wps[1].header.frame_id = frame_id;
    // TODO: 填写 position (米)
    wps[1].pose.position.x = -0.0139797;
    wps[1].pose.position.y = 0.0;
    wps[1].pose.position.z = 0.0973281;
    // TODO: 填写 orientation 四元数 (x,y,z,w)
    wps[1].pose.orientation.x = 0.0;
    wps[1].pose.orientation.y = 0.461687;
    wps[1].pose.orientation.z = 0.0;
    wps[1].pose.orientation.w = 0.887043;

    // === 轨迹点 3 ===
    wps[2].header.frame_id = frame_id;
    // TODO: 填写 position (米)
    wps[2].pose.position.x = -0.0732629;
    wps[2].pose.position.y = 0.0;
    wps[2].pose.position.z = 0.397173;
    // TODO: 填写 orientation 四元数 (x,y,z,w)
    wps[2].pose.orientation.x = 0.0;
    wps[2].pose.orientation.y = -0.250616;
    wps[2].pose.orientation.z = 0.0;
    wps[2].pose.orientation.w = 0.968087;

    // === 轨迹点 4 ===
    wps[3].header.frame_id = frame_id;
    // Position (米)
    wps[3].pose.position.x = 0.0978255;
    wps[3].pose.position.y = -0.293542;
    wps[3].pose.position.z = 0.263957;
    // Orientation (四元数)
    wps[3].pose.orientation.x = -0.0359773;
    wps[3].pose.orientation.y = -0.0831299;
    wps[3].pose.orientation.z = -0.466521;
    wps[3].pose.orientation.w = 0.87986;

    // === 轨迹点 5 ===
    wps[4].header.frame_id = frame_id;
    // Position (米)
    wps[4].pose.position.x = -0.0895236;
    wps[4].pose.position.y = 0.051642;
    wps[4].pose.position.z = 0.461902;
    // Orientation (四元数)
    wps[4].pose.orientation.x = 0.00926192;
    wps[4].pose.orientation.y = -0.290106;
    wps[4].pose.orientation.z = 0.264611;
    wps[4].pose.orientation.w = 0.919638;

    // === 轨迹点 6 ===
    wps[5].header.frame_id = frame_id;
    // Position (米)
    wps[5].pose.position.x = 0.202946;
    wps[5].pose.position.y = -0.0838137;
    wps[5].pose.position.z = 0.420467;
    // Orientation (四元数)
    wps[5].pose.orientation.x = -0.0242783;
    wps[5].pose.orientation.y = -0.153718;
    wps[5].pose.orientation.z = -0.114514;
    wps[5].pose.orientation.w = 0.981157;

    // === 轨迹点 7 ===
    wps[6].header.frame_id = frame_id;
    // Position (米)
    wps[6].pose.position.x = -0.00405299;
    wps[6].pose.position.y = -0.0128192;
    wps[6].pose.position.z = 0.148163;
    // Orientation (四元数)
    wps[6].pose.orientation.x = -0.254269;
    wps[6].pose.orientation.y = 0.334253;
    wps[6].pose.orientation.z = 0.0705149;
    wps[6].pose.orientation.w = 0.904793;

    // === 轨迹点 8 ===
    wps[7].header.frame_id = frame_id;
    // Position (米)
    wps[7].pose.position.x = 0.149384;
    wps[7].pose.position.y = -0.0469608;
    wps[7].pose.position.z = 0.330484;
    // Orientation (四元数)
    wps[7].pose.orientation.x = -0.0272986;
    wps[7].pose.orientation.y = -0.0693352;
    wps[7].pose.orientation.z = -0.00884794;
    wps[7].pose.orientation.w = 0.997181;

    // === 轨迹点 9 ===
    wps[8].header.frame_id = frame_id;
    // Position (米)
    wps[8].pose.position.x = -0.188134;
    wps[8].pose.position.y = 0.0908398;
    wps[8].pose.position.z = 0.406588;
    // Orientation (四元数)
    wps[8].pose.orientation.x = -0.1378;
    wps[8].pose.orientation.y = -0.441365;
    wps[8].pose.orientation.z = 0.254287;
    wps[8].pose.orientation.w = 0.849439;

    // === 轨迹点 10 ===
    wps[9].header.frame_id = frame_id;
    // Position (米)
    wps[9].pose.position.x = 0.138346;
    wps[9].pose.position.y = 0.0110588;
    wps[9].pose.position.z = 0.35104;
    // Orientation (四元数)
    wps[9].pose.orientation.x = 0.00247592;
    wps[9].pose.orientation.y = -0.194721;
    wps[9].pose.orientation.z = 0.0228966;
    wps[9].pose.orientation.w = 0.980588;

    return wps;
  }

  void tick()
  {
    if (busy_.exchange(true)) return;

    auto target = waypoints_[idx_];
    target.header.stamp = node_->now();
    idx_ = (idx_ + 1) % waypoints_.size();

    move_group_->setStartStateToCurrentState();
    move_group_->setPoseReferenceFrame(planning_frame_);
    move_group_->setPoseTarget(target.pose);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    const bool ok = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

    if (ok) {
      move_group_->execute(plan);
    }
    move_group_->clearPoseTargets();
    busy_ = false;
  }

  rclcpp::Node::SharedPtr node_;
  std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::string planning_frame_;
  std::vector<geometry_msgs::msg::PoseStamped> waypoints_;
  std::atomic<bool> busy_;
  std::size_t idx_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("zc4_periodic_planner");
  std::string group_name = node->declare_parameter<std::string>("planning_group", "zc4");

  auto app = std::make_shared<PeriodicMover>(node, group_name);
  rclcpp::executors::SingleThreadedExecutor exec;
  exec.add_node(node);
  exec.spin();

  rclcpp::shutdown();
  return 0;
}
